/**
 * @file /src/main_window.cpp
 *
 * @brief Implementation for the qt gui->
 *
 * @date February 2011
 **/
/*****************************************************************************
** Includes
*****************************************************************************/

#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/cyrobot_monitor/main_window.hpp"


/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace cyrobot_monitor {

using namespace Qt;

/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/

MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
	: QMainWindow(parent),
    ui(new Ui::MainWindowDesign), 
    qnode(argc,argv)
{
    ui->setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
    //QObject::connect(ui->actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp is a global variable for the application
    initUis();
    initData();
    //读取配置文件
    ReadSettings();
    setWindowIcon(QIcon(":/images/package.png"));
    ui->tab_manager->setCurrentIndex(0); // ensure the first tab is showing - qt-designer should have this already hardwired, but often loses it (settings?).
    //QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close()));
    
    /*********************
    ** Logging
    **********************/
    //ui->view_logging->setModel(qnode.loggingModel());
    
    addtopic_form = new AddTopics();
    //绑定添加rviz话题信号
    connect(addtopic_form, SIGNAL(Topic_choose(QTreeWidgetItem *, QString)), this, SLOT(slot_choose_topic(QTreeWidgetItem *, QString)));
    
    /*********************
    ** 自动连接master
    **********************/
    if ( ui->checkbox_remember_settings->isChecked() ) {
        on_button_connect_clicked();
    }
    //链接connect
    connections();



}

//析构函数
MainWindow::~MainWindow()
{
    if( base_cmd)
    {
        delete base_cmd;
        base_cmd = nullptr;
    }
    if(map_rviz_)
    {
        delete map_rviz_;
        map_rviz_ = nullptr;
    }
}

//初始化UI
void MainWindow::initUis()
{
    ui->groupBox_3->setEnabled(false);
    /*
    m_DashBoard_x =new CCtrlDashBoard(ui->widget_speed_x);
    m_DashBoard_x->setGeometry(ui->widget_speed_x->rect());
    m_DashBoard_x->setValue(0);
    m_DashBoard_y =new CCtrlDashBoard(ui->widget_speed_y);
    m_DashBoard_y->setGeometry(ui->widget_speed_y->rect());
    m_DashBoard_y->setValue(0);
    */
    ui->tab_manager->setCurrentIndex(0);
    ui->tabWidget->setCurrentIndex(0);

    //add zxp
    ui->tab_manager->setTabEnabled(0,false);
    ui->tabWidget->setTabEnabled(1,false);
    
    ui->pushButton_remove_topic->setEnabled(false);
    ui->pushButton_rename_topic->setEnabled(false);

    //qucik treewidget
    //ui->treeWidget_quick_cmd->setHeaderLabels(QStringList()<<"key"<<"values");
    //ui->treeWidget_quick_cmd->setHeaderHidden(true);
    
    ui->button_disconnect->setEnabled(false);
}

void MainWindow::initData()
{
    m_mapRvizDisplays.insert("Axes", RVIZ_DISPLAY_AXES);
    m_mapRvizDisplays.insert("Camera", RVIZ_DISPLAY_CAMERA);
    m_mapRvizDisplays.insert("DepthCloud", RVIZ_DISPLAY_DEPTHCLOUD);
    m_mapRvizDisplays.insert("Effort", RVIZ_DISPLAY_EFFORT);
    m_mapRvizDisplays.insert("FluidPressure", RVIZ_DISPLAY_FLUIDPRESSURE);
    m_mapRvizDisplays.insert("Grid", RVIZ_DISPLAY_GRID);
    m_mapRvizDisplays.insert("GridCells", RVIZ_DISPLAY_GRIDCELLS);
    m_mapRvizDisplays.insert("Group", RVIZ_DISPLAY_GROUP);
    m_mapRvizDisplays.insert("Illuminance", RVIZ_DISPLAY_ILLUMINANCE);
    m_mapRvizDisplays.insert("Image", RVIZ_DISPLAY_IMAGE);
    m_mapRvizDisplays.insert("InterativerMarker", RVIZ_DISPLAY_INTERATIVEMARKER);
    m_mapRvizDisplays.insert("LaserScan", RVIZ_DISPLAY_LASERSCAN);
    m_mapRvizDisplays.insert("Map", RVIZ_DISPLAY_MAP);
    m_mapRvizDisplays.insert("Marker", RVIZ_DISPLAY_MARKER);
    m_mapRvizDisplays.insert("MarkerArray", RVIZ_DISPLAY_MARKERARRAY);
    m_mapRvizDisplays.insert("Odometry", RVIZ_DISPLAY_ODOMETRY);
    m_mapRvizDisplays.insert("Path", RVIZ_DISPLAY_PATH);
    m_mapRvizDisplays.insert("PointCloud", RVIZ_DISPLAY_POINTCLOUD);
    m_mapRvizDisplays.insert("PointCloud2", RVIZ_DISPLAY_POINTCLOUD2);
    m_mapRvizDisplays.insert("PointStamped", RVIZ_DISPLAY_POINTSTAMPED);
    m_mapRvizDisplays.insert("Polygon", RVIZ_DISPLAY_POLYGON);
    m_mapRvizDisplays.insert("Pose", RVIZ_DISPLAY_POSE);
    m_mapRvizDisplays.insert("PoseArray", RVIZ_DISPLAY_POSEARRAY);
    m_mapRvizDisplays.insert("PoseWithCovariance", RVIZ_DISPLAY_POSEWITHCOVARIANCE);
    m_mapRvizDisplays.insert("Range", RVIZ_DISPLAY_RANGE);
    m_mapRvizDisplays.insert("RelativeHumidity", RVIZ_DISPLAY_RELATIVEHUMIDITY);
    m_mapRvizDisplays.insert("RobotModel", RVIZ_DISPLAY_ROBOTMODEL);
    m_mapRvizDisplays.insert("TF", RVIZ_DISPLAY_TF);
    m_mapRvizDisplays.insert("Temperature", RVIZ_DISPLAY_TEMPERATURE);
    m_mapRvizDisplays.insert("WrenchStamped", RVIZ_DISPLAY_WRENCHSTAMPED);
}

//订阅video话题
void MainWindow::initVideos()
{



}
void MainWindow::slot_show_image(int frame_id, QImage image)
{

}


void MainWindow::initRviz()
{
    ui->label_rvizShow->hide();
    map_rviz_=new QRviz(ui->verticalLayout_build_map,"qrviz");
    connect(map_rviz_, &QRviz::ReturnModelSignal, this, &MainWindow::RvizGetModel);
    map_rviz_->GetDisplayTreeModel();
    QMap<QString, QVariant> namevalue;
    namevalue.insert("Line Style", "Billboards");
    namevalue.insert("Color", QColor(160, 160, 160));
    namevalue.insert("Plane Cell Count", 10);
    map_rviz_->DisplayInit(RVIZ_DISPLAY_GRID, "Grid", true, namevalue);
    
    ui->pushButton_add_topic->setEnabled(true);
    ui->pushButton_rvizReadDisplaySet->setEnabled(true);
    ui->pushButton_rvizSaveDisplaySet->setEnabled(true);

    //add zxp
    QString path = "default.yaml";
    if (!path.isEmpty())
    {
        map_rviz_->ReadDisplaySet(path);
        ui->label_staue->setText("rviz配置导入成功!");
    }
    else
    {
        ui->label_staue->setText("rviz配置导入失败!");
    }


}

void MainWindow::RvizGetModel(QAbstractItemModel *model)
{
    m_modelRvizDisplay = model;
    ui->treeView_rvizDisplayTree->setModel(model);
}

void MainWindow::connections()
{
    QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(slot_rosShutdown()));
    QObject::connect(&qnode, SIGNAL(Master_shutdown()), this, SLOT(slot_rosShutdown()));
    //connect速度的信号
    connect(&qnode,SIGNAL(speed_x(double)),this,SLOT(slot_speed_x(double)));
    connect(&qnode,SIGNAL(speed_y(double)),this,SLOT(slot_speed_y(double)));
    //机器人状态
    connect(&qnode,SIGNAL(power(float)),this,SLOT(slot_power(float)));
    //电源的信号
    connect(&qnode,SIGNAL(robot_control_status(qint8)),this,SLOT(slot_robot_control_status(qint8)));
    //机器人位置信号
    connect(&qnode,SIGNAL(position(QString,double,double,double,double)),this,SLOT(slot_position_change(QString,double,double,double,double)));
    //导航状态
    connect(&qnode,SIGNAL(movebase_status(qint8)),this,SLOT(slot_move_base_status(qint8)));
    //机器人log
    connect(&qnode,SIGNAL(RosLog_string(qint8,QString)),this,SLOT(slot_roslog_msg(qint8,QString)));
    //绑定快捷按钮相关函数
    //connect(ui->quick_cmd_add_btn,SIGNAL(clicked()),this,SLOT(quick_cmd_add()));
    //connect(ui->quick_cmd_remove_btn,SIGNAL(clicked()),this,SLOT(quick_cmd_remove()));
    //绑定slider的函数
    //connect(ui->horizontalSlider_raw,SIGNAL(valueChanged(int)),this,SLOT(on_Slider_raw_valueChanged(int)));
    //connect(ui->horizontalSlider_linear,SIGNAL(valueChanged(int)),this,SLOT(on_Slider_linear_valueChanged(int)));
    //设置界面
    connect(ui->action_2,SIGNAL(triggered(bool)),this,SLOT(slot_setting_frame()));
    //绑定速度控制按钮
    connect(ui->pushButton_i,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_u,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_o,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_j,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_l,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_m,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_back,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    connect(ui->pushButton_backr,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
    //设置2D Pose
    connect(ui->set_pos_btn,SIGNAL(clicked()),this,SLOT(slot_set_2D_Pos()));
    //设置2D goal
    connect(ui->set_goal_btn,SIGNAL(clicked()),this,SLOT(slot_set_2D_Goal()));
    //设置Punlish Point
    connect(ui->set_publish_btn,SIGNAL(clicked()),this,SLOT(slot_set_Publisg_Point()));
    //设置MoveCamera
    connect(ui->move_camera_btn,SIGNAL(clicked()),this,SLOT(slot_move_camera_btn()));
    //设置Select
    connect(ui->set_select_btn,SIGNAL(clicked()),this,SLOT(slot_set_select()));
    //设置返航点
    connect(ui->set_return_btn,SIGNAL(clicked()),this,SLOT(slot_set_return_point()));
    //返航
    connect(ui->return_btn,SIGNAL(clicked()),this,SLOT(slot_return_point()));
    //设置巡航点
    connect(ui->set_cruise_btn,SIGNAL(clicked()),this,SLOT(slot_set_cruise_Point()));
    //左工具栏tab索引改变
    connect(ui->tab_manager,SIGNAL(currentChanged(int)),this,SLOT(slot_tab_manage_currentChanged(int)));
    //右工具栏索引改变
    connect(ui->tabWidget,SIGNAL(currentChanged(int)),this,SLOT(slot_tab_Widget_currentChanged(int)));
    //刷新话题列表
    connect(ui->refreash_topic_btn,SIGNAL(clicked()),this,SLOT(refreashTopicList()));
}
//设置界面
void MainWindow::slot_setting_frame()
{
    if(set != nullptr)
    {
        delete set;
        set=new Settings();
        set->setWindowModality(Qt::ApplicationModal);
        set->show();
    }
    else
    {
        set=new Settings();
        set->setWindowModality(Qt::ApplicationModal);
        set->show();
    }
    //绑定set确认按钮点击事件
}
//刷新当前坐标
void MainWindow::slot_position_change(QString frame,double x,double y,double z,double w)
{
    //更新ui显示
    ui->label_frame->setText(frame);
    ui->label_x->setText(QString::number(x));
    ui->label_y->setText(QString::number(y));
    ui->label_z->setText(QString::number(z));
    ui->label_w->setText(QString::number(w));
}
//刷新返航地点
void MainWindow::slot_set_return_point()
{
    //更新ui返航点显示
    ui->label_return_x->setText(ui->label_x->text());
    ui->label_return_y->setText(ui->label_y->text());
    ui->label_return_z->setText(ui->label_z->text());
    ui->label_return_w->setText(ui->label_w->text());
    //写入setting
    QSettings settings("return-position", "cyrobot_rviz_tree");
    settings.setValue("x",ui->label_x->text());
    settings.setValue("y",ui->label_y->text());
    settings.setValue("z",ui->label_z->text());
    settings.setValue("w",ui->label_w->text());

    //add zxp
    ui->label_staue->setText("充电点设置成功!");
    //发送命令
    qnode.publish_robot_control("set_power_point");


}
//返航
void MainWindow::slot_return_point()
{
  //发送命令
  if(ui->return_btn->text() == "回充")
  {
      qnode.publish_robot_control("go_power_point");
  }
  else
  {
      qnode.publish_robot_control("cancel_go_charge");
  }


}
//设置导航当前位置按钮的槽函数
void MainWindow::slot_set_2D_Pos()
{
 map_rviz_->Set_Pos();
// ui->label_map_msg->setText("请在地图中选择机器人的初始位置");
}
//设置导航目标位置按钮的槽函数
void MainWindow::slot_set_2D_Goal()
{
  map_rviz_->Set_Goal();
//  ui->label_map_msg->setText("请在地图中选择机器人导航的目标位置");
}
//设置巡航点
void MainWindow::slot_set_cruise_Point()
{
  map_rviz_->Set_cruisePoint();
}
//设置publish point按钮的槽函数
void MainWindow::slot_set_Publisg_Point()
{
  map_rviz_->Set_PublisPoint();
//  ui->label_map_msg->setText("请在地图中选择机器人导航的目标位置");
}
void MainWindow::slot_move_camera_btn()
{
    map_rviz_->Set_MoveCamera();
    qDebug()<<"move camera";
}
void MainWindow::slot_set_select()
{
    map_rviz_->Set_Select();
}


//选中要添加的话题的槽函数
void MainWindow::slot_choose_topic(QTreeWidgetItem *choose, QString name)
{
    QString ClassID = choose->text(0);
    // 检查重名
    name = JudgeDisplayNewName(name);
    
    qDebug() << "choose topic ClassID:" << ClassID << ", name:" << name;
    QMap<QString, QVariant> namevalue;
    namevalue.clear();
    map_rviz_->DisplayInit(m_mapRvizDisplays[ClassID], name, true, namevalue);
}

///
/// \brief 检查重名
/// \param name
/// \return 
///
QString MainWindow::JudgeDisplayNewName(QString name)
{
    if (m_modelRvizDisplay != nullptr)
    {
        bool same = true;
        while (same)
        {
            same = false;
            for (int i = 0; i < m_modelRvizDisplay->rowCount(); i++)
            {
                //m_sRvizDisplayChooseName = index.data().value<QString>();
                if (m_modelRvizDisplay->index(i, 0).data().value<QString>() == name)
                {
                    if (name.indexOf("_") != -1)
                    {
                        int num = name.section("_", -1, -1).toInt();
                        name = name.left(name.length() - name.section("_", -1, -1).length() - 1);
                        if (num <= 1)
                        {
                            num = 2;
                        }
                        else
                        {
                            num++;
                        }
                        name = name + "_" + QString::number(num);
                    }
                    else {
                      name = name + "_2";
                    }
                    same = true;
                    break;
                }
            }
        }
    }
    return name;
}

//左工具栏索引改变
void MainWindow::slot_tab_manage_currentChanged(int index)
{
    switch (index) {
    case 0:
        //ui->tabWidget->setCurrentIndex(1);
        break;
    case 1:


        break;
    case 2:
        break;

    }
}
//右工具栏索引改变
void MainWindow::slot_tab_Widget_currentChanged(int index)
{
    switch (index) {
    case 0:

        break;
    case 1:
        ui->tab_manager->setCurrentIndex(0);
        break;
    case 2:
        break;

    }
}
//速度控制相关按钮处理槽函数
void MainWindow::slot_cmd_control()
{

    QPushButton* btn=qobject_cast<QPushButton*>(sender());
    char key=btn->text().toStdString()[0];
    //速度
    float liner=ui->horizontalSlider_linear->value()*0.01f;
    float turn=ui->horizontalSlider_raw->value()*0.01f;
    bool is_all=ui->checkBox_use_all->isChecked();
    switch (key) {
        case 'u':
            qnode.move_base(is_all?'U':'u',liner,turn);
        break;
        case 'i':
            qnode.move_base(is_all?'I':'i',liner,turn);
        break;
        case 'o':
            qnode.move_base(is_all?'O':'o',liner,turn);
        break;
        case 'j':
            qnode.move_base(is_all?'J':'j',liner,turn);
        break;
        case 'l':
            qnode.move_base(is_all?'L':'l',liner,turn);
        break;
        case 'm':
            qnode.move_base(is_all?'M':'m',liner,turn);
        break;
        case ',':
            qnode.move_base(is_all?'<':',',liner,turn);
        break;
        case '.':
            qnode.move_base(is_all?'>':'.',liner,turn);
        break;
    }
}
//滑动条处理槽函数
void MainWindow::on_Slider_raw_valueChanged(int v)
{
    ui->label_raw->setText(QString::number(v));
}
//滑动条处理槽函数
void MainWindow::on_Slider_linear_valueChanged(int v)
{
    ui->label_linear->setText(QString::number(v));
}
//快捷指令删除按钮
void MainWindow::quick_cmd_remove()
{

}
//快捷指令添加按钮
void MainWindow::quick_cmd_add()
{

}
//向treeWidget添加快捷指令
void MainWindow::add_quick_cmd(QString name,QString val)
{

}
//快捷指令按钮处理的函数
void MainWindow::quick_cmds_check_change(int state)
{

}
//执行一些命令的回显
void MainWindow::cmd_output()
{

}
//执行一些命令的错误回显
void MainWindow::cmd_error_output()
{

}



/*****************************************************************************
** Implementation [Slots]
*****************************************************************************/

void MainWindow::showNoMasterMessage() {
	QMessageBox msgBox;
	msgBox.setText("Couldn't find the ros master.");
	msgBox.exec();
    close();
}

void MainWindow::inform(QString strdata)
{
    QMessageBox m_r;
    m_r.setWindowTitle("提示");
    m_r.setText(strdata);
    m_r.exec();
}
bool MainWindow::AskInform(QString strdata)
{
    QMessageBox m_r;

    m_r.setWindowTitle("提示");
    m_r.setText(strdata);
    m_r.addButton(tr("确认"), QMessageBox::ActionRole);
    m_r.addButton(tr("取消"), QMessageBox::ActionRole);

    int isok = m_r.exec();
    if (isok == 1)
    {
        return false;
    }
    else
    {
        return true;
    }
}

/*
 * These triggers whenever the button is clicked, regardless of whether it
 * is already checked or not.
 */

void MainWindow::initTopicList()
{
    ui->topic_listWidget->clear();
    //ui->topic_listWidget->addItem(QString("%1   (%2)").arg("Name","Type"));
    QMap<QString,QString> topic_list= qnode.get_topic_list();


    for(QMap<QString,QString>::iterator iter=topic_list.begin();iter!=topic_list.end();iter++)
    {
       QListWidgetItem * Logitem = new QListWidgetItem;
       Logitem->setTextColor(Qt::white);
       Logitem->setText(QString("%1   (%2)").arg(iter.key(),iter.value()));
       ui->topic_listWidget->addItem(Logitem);
    }
}
void MainWindow::refreashTopicList()
{
    //add zxp
    if (map_rviz_ == nullptr)
    {
        return;
    }

    initTopicList();
}
//当ros与master的连接断开时
void MainWindow::slot_rosShutdown()
{
    ui->label_robot_staue_img->setPixmap(QPixmap::fromImage(QImage("://images/icon(4.12)/离线.png")));
     ui->label_statue_text->setStyleSheet("color:red;");
    ui->label_statue_text->setText("离线");
    ui->button_connect->setEnabled(true);
    ui->line_edit_master->setReadOnly(false);
    ui->line_edit_host->setReadOnly(false);
    ui->line_edit_topic->setReadOnly(false);
    //add zxp
    ui->tab_manager->setTabEnabled(0,false);
    ui->tabWidget->setTabEnabled(1,false);
    info_show("ros_showdown");
}
void MainWindow::slot_power(float p)
{
    ui->label_power->setText(QString::number(static_cast<double>(p)).mid(0,5)+"V");
    double n=(p - min_power)/(max_power - min_power);
    int value = static_cast<int>(n*100);
    ui->progressBar->setValue(value>100?100:value);

    //当电量低于设置的自动回充电压时开启自动回充
    if(n*100<=go_power && powe_check_status == true)
    {
        //ui->label_staue->setText("低电量开始自动回充!");
         //ui->progressBar->setStyleSheet("QProgressBar::chunk {background-color: red;width: 20px;} QProgressBar {border: 2px solid grey;border-radius: 5px;text-align: center;}");
         // QMessageBox::warning(NULL, "电量不足", "电量不足，请及时充电！", QMessageBox::Yes , QMessageBox::Yes);
    }
    else
    {
        //ui->progressBar->setStyleSheet("QProgressBar {border: 2px solid grey;border-radius: 5px;text-align: center;}");
    }
}

void MainWindow::slot_robot_control_status(qint8 status)
{
    qint8 tem_status = status;

    if(tem_status & 0x01)
    {
        ui->start_zkwl_btn->setText("关闭底盘");
        ui->start_zkwl_btn->setEnabled(true);
    }
    else
    {
        ui->start_zkwl_btn->setText("启动底盘");
        ui->start_zkwl_btn->setEnabled(true);

    }

    tem_status = status;
    if(tem_status & 0x02)
    {
        ui->start_nav_btn->setText("关闭导航");
        ui->start_nav_btn->setEnabled(true);

    }
    else
    {
        ui->start_nav_btn->setText("启动导航");
        ui->start_nav_btn->setEnabled(true);

    }

    tem_status = status;
    if(tem_status & 0x04)
    {
        ui->start_slam_bnt->setText("关闭建图");
        ui->start_slam_bnt->setEnabled(true);

    }
    else
    {
        ui->start_slam_bnt->setText("启动建图");
        ui->start_slam_bnt->setEnabled(true);

    }

    tem_status = status;
    if(tem_status & 0x08)
    {
        ui->return_btn->setText("关闭回充");

    }
    else
    {
        ui->return_btn->setText("回充");
    }

}


void MainWindow::slot_move_base_status(qint8 status)
{
    switch(status)
    {
        case 1:
        {
           ui->label_staue->setText("导航正在前往目标点");
           ui->start_move_base_pushButton->setText("结束巡航");
           break;
        }
        case 2:
        {
           ui->label_staue->setText("导航待机");
           ui->start_move_base_pushButton->setText("开启巡航");
           break;
        }
        case 3:
        {
           ui->label_staue->setText("导航成功到达目标点");
           //ui->start_move_base_pushButton->setText("结束巡航");
           break;
        }
        case 4:
        {
           ui->label_staue->setText("导航失败");
           ui->start_move_base_pushButton->setText("开启巡航");
           break;
        }
        case 6:
        {
           ui->label_staue->setText("导航取消");
           ui->start_move_base_pushButton->setText("开启巡航");
           break;
        }


        default:
        {
           ui->label_staue->setText("导航错误");
           ui->start_move_base_pushButton->setText("开启巡航");
           break;
        }
    }
}

void MainWindow::slot_roslog_msg(qint8 level, QString msg)
{
    QListWidgetItem * Logitem = new QListWidgetItem;

    switch(level)
    {
      case 1:
      {
        Logitem->setTextColor(Qt::blue);
        break;
      }
      case 2:
      {
        Logitem->setTextColor(Qt::green);
        break;
      }
      case 4:
      {
        Logitem->setTextColor(Qt::yellow);
        break;
      }
      case 8:
      {
        Logitem->setTextColor(Qt::red);
        break;
      }
      case 16:
      {
        Logitem->setTextColor(Qt::red);
        break;
      }
    }

    //get time
    QTime qtimeObj = QTime::currentTime();
    QString strTime = qtimeObj.toString("hh:mm:ss ");
    strTime.append("  :  ");
    strTime.append(msg);
    //add timer data
    Logitem->setText(strTime);

    //show
    //最大限制500行
    if(ui->info_listWidget->count() > 500)
    {
      ui->info_listWidget->takeItem(0);
    }
    ui->info_listWidget->addItem(Logitem);
    ui->info_listWidget->scrollToBottom();

}

void MainWindow::slot_speed_x(double x)
{
    //QString::number(x,'f',2); // f 表示非科学记数法  2表示小数点后保留10位
    ui->label_Vx->setText(QString::number(x,'f',2)+"m/s");
}
void MainWindow::slot_speed_y(double x)
{
    //QString::number(x,'f',2); // f 表示非科学记数法  2表示小数点后保留10位
    ui->label_Vz->setText(QString::number(x,'f',2)+"m/s");
}
void MainWindow::on_checkbox_use_environment_stateChanged(int state) {
	bool enabled;
	if ( state == 0 ) {
		enabled = true;
	} else {
		enabled = false;
	}
	ui->line_edit_master->setEnabled(enabled);
	ui->line_edit_host->setEnabled(enabled);
	//ui->line_edit_topic->setEnabled(enabled);
}

/*****************************************************************************
** Implementation [Menu]
*****************************************************************************/

void MainWindow::on_actionAbout_triggered() {
    //QMessageBox::about(this, tr("About ..."),tr("<h2>PACKAGE_NAME Test Program 0.10</h2><p>Copyright Yujin Robot</p><p>This package needs an about description.</p>"));
}

/*****************************************************************************
** Implementation [Configuration]
*****************************************************************************/

void MainWindow::ReadSettings() {
    QSettings settings("Qt-Ros Package", "cyrobot_rviz_tree");
    restoreGeometry(settings.value("geometry").toByteArray());
    restoreState(settings.value("windowState").toByteArray());
    QString master_url = settings.value("master_url",QString("http://192.168.1.2:11311/")).toString();
    QString host_url = settings.value("host_url", QString("192.168.1.3")).toString();
    //QString topic_name = settings.value("topic_name", QString("/chatter")).toString();
    ui->line_edit_master->setText(master_url);
    ui->line_edit_host->setText(host_url);
    //ui->line_edit_topic->setText(topic_name);
    bool remember = settings.value("remember_settings", false).toBool();
    ui->checkbox_remember_settings->setChecked(remember);
    bool checked = settings.value("use_environment_variables", false).toBool();
    ui->checkbox_use_environment->setChecked(checked);
    if ( checked ) {
    	ui->line_edit_master->setEnabled(false);
    	ui->line_edit_host->setEnabled(false);
    	//ui->line_edit_topic->setEnabled(false);
    }

    QSettings return_pos("return-position","cyrobot_rviz_tree");
    ui->label_return_x->setText(return_pos.value("x",QString("0")).toString());
    ui->label_return_y->setText(return_pos.value("y",QString("0")).toString());
    ui->label_return_z->setText(return_pos.value("z",QString("0")).toString());
    ui->label_return_w->setText(return_pos.value("w",QString("0")).toString());

    //读取快捷指令的setting
    /*
    QSettings quick_setting("quick_setting","cyrobot_rviz_tree");
    QStringList ch_key=quick_setting.childKeys();
    for(auto c:ch_key)
    {
        add_quick_cmd(c,quick_setting.value(c,QString("")).toString());
    }*/
    //add zxp 读取回充配置
    QSettings power_setting("gopower_setting","cyrobot_monitor");
    go_power = power_setting.value("go_power","20").toDouble();
    powe_check_status = power_setting.value("power_check","false").toBool();

}

void MainWindow::WriteSettings() {
    QSettings settings("Qt-Ros Package", "cyrobot_rviz_tree");
    settings.setValue("master_url",ui->line_edit_master->text());
    settings.setValue("host_url",ui->line_edit_host->text());
    //settings.setValue("topic_name",ui->line_edit_topic->text());
    settings.setValue("use_environment_variables",QVariant(ui->checkbox_use_environment->isChecked()));
    settings.setValue("geometry", saveGeometry());
    //settings.setValue("windowState", saveState());
    settings.setValue("remember_settings",QVariant(ui->checkbox_remember_settings->isChecked()));


    //存下快捷指令的setting
    QSettings quick_setting("quick_setting","cyrobot_rviz_tree");
    quick_setting.clear();
    /*
    for(int i=0;i<ui->treeWidget_quick_cmd->topLevelItemCount();i++)
    {
        QTreeWidgetItem *top=ui->treeWidget_quick_cmd->topLevelItem(i);
        QTextEdit *cmd_val= static_cast<QTextEdit *>(ui->treeWidget_quick_cmd->itemWidget(top->child(0),1));
        quick_setting.setValue(top->text(0),cmd_val->toPlainText());
    }*/
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    WriteSettings();
    QMainWindow::closeEvent(event);
}

}  // namespace cyrobot_monitor

///
/// \brief 增加topic
///
void cyrobot_monitor::MainWindow::on_pushButton_add_topic_clicked()
{
    addtopic_form->show();
}

///
/// \brief 删除topic
///
void cyrobot_monitor::MainWindow::on_pushButton_remove_topic_clicked()
{
    if (ui->treeView_rvizDisplayTree->currentIndex().row() >= 0)
    {
        m_sRvizDisplayChooseName_ = ui->treeView_rvizDisplayTree->currentIndex().data().value<QString>();
        map_rviz_->RemoveDisplay(m_sRvizDisplayChooseName_);
        if (ui->treeView_rvizDisplayTree->currentIndex().row() >= 0)
        {
            on_treeView_rvizDisplayTree_clicked(ui->treeView_rvizDisplayTree->currentIndex());
        }
        else
        {
            m_sRvizDisplayChooseName_.clear();
        }
    }
    else
    {
        inform("请选择Display后再执行此操作");
    }
}

///
/// \brief 重命名topic
///
void cyrobot_monitor::MainWindow::on_pushButton_rename_topic_clicked()
{
    if (ui->treeView_rvizDisplayTree->currentIndex().row() < 0)
    {
        inform("请选择Display后再执行此操作");
        return ;
    }
    QString dlgTitle = "重命名";
    QString txtlabel = "请输入名字：";
    QString defaultInupt = m_sRvizDisplayChooseName_;
    QLineEdit::EchoMode echoMode = QLineEdit::Normal;
    bool ok = false;
    QString newname = QInputDialog::getText(this, dlgTitle, txtlabel, echoMode, defaultInupt, &ok);
    if (ok && !newname.isEmpty())
    {
        if (newname != defaultInupt)
        {
            QString nosamename = JudgeDisplayNewName(newname);
            map_rviz_->RenameDisplay(defaultInupt, nosamename);
            m_sRvizDisplayChooseName_ = nosamename;
            if (nosamename != newname)
            {
                inform("命名重复！命名已自动更改为" + nosamename);
            }
        }
    }
    else if (ok)
    {
        inform("输入内容为空，重命名失败");
    }
}

void cyrobot_monitor::MainWindow::on_treeView_rvizDisplayTree_clicked(const QModelIndex &index)
{
    m_sRvizDisplayChooseName_ = index.data().value<QString>();
    if (index.parent().row() == -1)   // Display 的根节点
    {
        if (index.row() > 1)
        {
            ui->pushButton_remove_topic->setEnabled(true);
            ui->pushButton_rename_topic->setEnabled(true);
        }
        else
        {
            ui->pushButton_remove_topic->setEnabled(false);
            ui->pushButton_rename_topic->setEnabled(false);
        }
    }
    else
    {
        ui->pushButton_remove_topic->setEnabled(false);
        ui->pushButton_rename_topic->setEnabled(false);
    }
}

/// \brief 连接ROS
void cyrobot_monitor::MainWindow::on_button_connect_clicked()
{
    //如果使用环境变量
    if ( ui->checkbox_use_environment->isChecked() )
    {
        if ( !qnode.init() )
        {
            //showNoMasterMessage();
            QMessageBox::warning(nullptr, "失败", "连接ROS Master失败！请检查你的网络或连接字符串！", QMessageBox::Yes, QMessageBox::Yes);
            ui->label_robot_staue_img->setPixmap(QPixmap::fromImage(QImage("://images/icon(4.12)/离线.png")));
            ui->label_statue_text->setStyleSheet("color:red;");
            ui->label_statue_text->setText("离线");
            ui->groupBox_3->setEnabled(false);
            //add zxp
            ui->treeView_rvizDisplayTree->setEnabled(false);
            ui->tab_manager->setTabEnabled(1,false);
            ui->tabWidget->setTabEnabled(1,false);
            return ;
        }
    }
    //如果不使用环境变量
    else
    {
        if ( !qnode.init(ui->line_edit_master->text().toStdString(), ui->line_edit_host->text().toStdString()) )
        {
            QMessageBox::warning(nullptr, "失败", "连接ROS Master失败！请检查你的网络或连接字符串！", QMessageBox::Yes, QMessageBox::Yes);
            ui->label_robot_staue_img->setPixmap(QPixmap::fromImage(QImage("://images/icon(4.12)/离线.png")));
            ui->label_statue_text->setStyleSheet("color:red;");
            ui->label_statue_text->setText("离线");

            ui->groupBox_3->setEnabled(false);
            //showNoMasterMessage();
            //add zxp
            ui->treeView_rvizDisplayTree->setEnabled(false);
            //add zxp
            ui->tab_manager->setTabEnabled(0,false);
            ui->tabWidget->setTabEnabled(1,false);
            return ;
        }
        else
        {
            ui->line_edit_master->setReadOnly(true);
            ui->line_edit_host->setReadOnly(true);
            ui->line_edit_topic->setReadOnly(true);
        }
    }
    //add zxp
    ui->tab_manager->setTabEnabled(0,true);
    ui->tabWidget->setTabEnabled(1,true);
    ui->groupBox_3->setEnabled(true);
    info_show("ros_connect");

    //add zxp
    ui->treeView_rvizDisplayTree->setEnabled(true);
    //初始化rviz
    initRviz();
    
    
    ui->button_connect->setEnabled(false);
    ui->label_robot_staue_img->setPixmap(QPixmap::fromImage(QImage("://images/icon(4.12)/在线.png")));
    ui->label_statue_text->setStyleSheet("color:green;");
    ui->label_statue_text->setText("在线");
    ui->button_disconnect->setEnabled(true);
    //初始化视频订阅的显示
    //initVideos();
    //显示话题列表
    initTopicList();
    //add zxp
    ui->start_zkwl_btn->setEnabled(true);
    ui->start_nav_btn->setEnabled(true);
    ui->start_slam_bnt->setEnabled(true);
    ui->save_map_btn->setEnabled(true);
    //get set power
    QSettings main_setting("topic_setting","cyrobot_monitor");
    min_power = main_setting.value("power_min","21").toDouble();
    max_power = main_setting.value("power_max","24").toDouble();
    //pub power
    QSettings power_setting("gopower_setting","cyrobot_monitor");

    qnode.publish_robot_control("set_go_power:"+main_setting.value("power_min","21").toString()+":"+main_setting.value("power_max","24").toString()
                                +":"+power_setting.value("go_power","20").toString()+":"+power_setting.value("power_check","0").toString());


}

/// \brief 断开ROS
void cyrobot_monitor::MainWindow::on_button_disconnect_clicked()
{
    qnode.disinit();
    map_rviz_->quit();
    delete map_rviz_;
    map_rviz_ = nullptr;
    ui->pushButton_add_topic->setEnabled(false);
    ui->pushButton_remove_topic->setEnabled(false);
    ui->pushButton_rename_topic->setEnabled(false);
    ui->pushButton_rvizReadDisplaySet->setEnabled(false);
    ui->pushButton_rvizSaveDisplaySet->setEnabled(false);
    ui->label_rvizShow->show();
    ui->button_disconnect->setEnabled(false);
    ui->button_connect->setEnabled(true);
    //add zxp
    slot_rosShutdown();
    ui->start_zkwl_btn->setEnabled(false);
    ui->start_nav_btn->setEnabled(false);
    ui->start_slam_bnt->setEnabled(false);
    ui->save_map_btn->setEnabled(false);

}

void cyrobot_monitor::MainWindow::on_pushButton_rvizReadDisplaySet_clicked()
{
    if (map_rviz_ == nullptr)
    {
        return;
    }
    //QString path = QFileDialog::getOpenFileName(this, "导入 RVIZ Display 配置", "/home/" + getUserName() + "/", "YAML(*.yaml);;ALL(*.*)");
    QString path = QFileDialog::getOpenFileName(this, "导入 RVIZ Display 配置", "./src/ros_qt5_gui_app_zkwl_rviz/", "YAML(*.yaml);;ALL(*.*)");
    if (!path.isEmpty())
    {
        map_rviz_->ReadDisplaySet(path);
    }
}

//add zxp
//save rviz set
void cyrobot_monitor::MainWindow::on_pushButton_rvizSaveDisplaySetIn_clicked()
{
    if (map_rviz_ == nullptr)
    {
        return;
    }
    //QString path = "./src/ros_qt5_gui_app_zkwl_rviz/default.yaml";
    QString path = "default.yaml";

    if (!path.isEmpty())
    {
        map_rviz_->OutDisplaySet(path);
        ui->label_staue->setText("保存成功!");
    }
    else
    {
      ui->label_staue->setText("保存失败!");
    }
}

void cyrobot_monitor::MainWindow::on_pushButton_rvizSaveDisplaySet_clicked()
{
    if (map_rviz_ == nullptr)
    {
        return;
    }
    QString path = QFileDialog::getSaveFileName(this, "导出 RVIZ Display 配置", "/home/" + getUserName() + "/", "YAML(*.yaml)");
    
    if (!path.isEmpty())
    {
        if (path.section('/', -1, -1).indexOf('.') < 0)
        {
            path = path + ".yaml";
        }
        map_rviz_->OutDisplaySet(path);
    }
}

QString cyrobot_monitor::MainWindow::getUserName()
{
    QString userPath = QStandardPaths::writableLocation(QStandardPaths::HomeLocation);
    QString userName = userPath.section("/", -1, -1);
    return userName;
}

//add zxp
void cyrobot_monitor::MainWindow::info_show(QString str)
{
    //get time
    QTime qtimeObj = QTime::currentTime();
    QString strTime = qtimeObj.toString("hh:mm:ss ");
    strTime.append("  :  ");
    strTime.append(str);
    //show
    QListWidgetItem * Logitem = new QListWidgetItem;
    Logitem->setTextColor(Qt::white);
    Logitem->setText(strTime);

    ui->info_listWidget->addItem(Logitem);
    ui->info_listWidget->scrollToBottom();
}




//start zkwl robot
void cyrobot_monitor::MainWindow::on_start_zkwl_btn_clicked()
{

    if(ui->start_zkwl_btn->text() == "启动底盘")
    {
        info_show("start_zkwl");
        qnode.publish_robot_control("start_zkwl");
        ui->start_zkwl_btn->setEnabled(false);
    }
    else
    {
        info_show("kill_zkwl");
        qnode.publish_robot_control("kill_zkwl");
        ui->start_zkwl_btn->setEnabled(false);
    }

}

//start nav
void cyrobot_monitor::MainWindow::on_start_nav_btn_clicked()
{

    if(ui->start_nav_btn->text() == "启动导航")
    {
        info_show("start_nav");
        qnode.publish_robot_control("start_nav");
        ui->start_nav_btn->setEnabled(false);
    }
    else
    {
        info_show("kill_nav");
        qnode.publish_robot_control("kill_nav");
        ui->start_nav_btn->setEnabled(false);
    }

}

//start slam
void cyrobot_monitor::MainWindow::on_start_slam_bnt_clicked()
{
  /*
    //创建JSON对象
    QJsonObject root_Obj;
    //添加键值对，值的类型自动识别，顺序不可自定义
    root_Obj.insert("cmd_type", "start");
    root_Obj.insert("zkwl_robot", "Run");
    //创建Json文档
    QJsonDocument root_Doc;
    root_Doc.setObject(root_Obj);
    QByteArray root_str = root_Doc.toJson(QJsonDocument::Indented);   //标准JSON格式    QString strJson(root_str);
    QString strJson(root_str);
    qDebug() << strJson;
    qnode.publish_robot_control(strJson.toStdString().data());
    */

    if(ui->start_slam_bnt->text() == "启动建图")
    {
        info_show("start_slam");
        qnode.publish_robot_control("start_slam");
        ui->start_slam_bnt->setEnabled(false);
    }
    else
    {
        info_show("kill_slam");
        qnode.publish_robot_control("kill_slam");
        ui->start_slam_bnt->setEnabled(false);
    }

}

//save map
void cyrobot_monitor::MainWindow::on_save_map_btn_clicked()
{
    info_show("save_map");
    qnode.publish_robot_control("save_map");
}
// clear curis point
void cyrobot_monitor::MainWindow::on_Button_clear_curspoint_clicked()
{
    info_show("clear_cruisepoint");
    qnode.clear_cruisepoint();

}
// start move base
void cyrobot_monitor::MainWindow::on_start_move_base_pushButton_clicked()
{

  if(ui->start_nav_btn->text() == "关闭导航" && ui->start_move_base_pushButton->text() == "开启巡航")
  {
      qnode.publish_robot_control("start_move_base:"+ui->lineEdit_curspoint_time->text());
      ui->start_move_base_pushButton->setText("结束巡航");
  }
  else if(ui->start_move_base_pushButton->text() == "结束巡航")
  {
      qnode.publish_robot_control("stop_move_base");
      ui->start_move_base_pushButton->setText("开启巡航");
  }


}

//clear ros log
void cyrobot_monitor::MainWindow::on_clear_ros_log_btn_clicked()
{
    ui->info_listWidget->clear();
}
